/*!
  @file autorun.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "mrover_vo/vis_odometer.h"
#include "mrover_vo/camera_params.h"


template <typename ValType>
void convertCV2Eigen(const cv::Mat &m, Eigen::MatrixXf &e) {
  e.resize(m.rows, m.cols);
  for (int i = 0; i < m.rows; ++i)
    for (int j = 0; j < m.cols; ++j)
      e(i, j) = m.at<ValType>(i, j);
}


void writeToFile(const std::string &filename, const cv::Mat &m) {
  Eigen::MatrixXf T;
  convertCV2Eigen<float>(m, T);
  Eigen::Quaternionf q(T.block<3, 3>(0, 0));
  cv::Mat r = m(cv::Rect(0, 0, 3, 3));

  std::ofstream ofs(filename.c_str(), std::ios_base::app);
  if (ofs.is_open()) {
    ofs << "cor";
    for (int i = 0; i < 4; ++i) {
      ofs << q.vec()[i] << "\t";
    }
    for (int i = 0; i < 3; ++i) {
      ofs << T(i, 3) << "\t";
    }
    ofs << std::endl;
    ofs.close();
  }
}

int main(int ac, char **av) {

  ros::init(ac, av, "autorun");

  std::string imgL_list_;
  std::string imgR_list_;
  std::string lineL, lineR;
  cv::Mat imgL, imgR;
  std::string pose_out_;

  vo::VisOdometer::Ptr visod_(new vo::VisOdometer);

  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_("~");
  private_nh_.param<std::string>("imageL_list", imgL_list_, "imgL");
  private_nh_.param<std::string>("imageR_list", imgR_list_, "imgR");
  private_nh_.param<std::string>("pose_out", pose_out_, "pose.txt");
  std::ifstream ifsL(imgL_list_.c_str(), std::ifstream::in);
  std::ifstream ifsR(imgR_list_.c_str(), std::ifstream::in);
  if (!ifsL.is_open() || !ifsR.is_open()) {
    ROS_ERROR("File open");
    ROS_BREAK();
  }
  std::ofstream ofs(pose_out_.c_str(), std::ios_base::out);
  ofs.close();  // delete file content

  vo::CameraParams::Ptr cam_params_(new vo::CameraParams);
  std::string camera_yaml;
  private_nh_.param<std::string>("camera_yaml", camera_yaml, "");
  if(camera_yaml.empty()) ROS_ERROR("Please specify 'camera_yaml' parameter.");
  else cam_params_->initFromFile(camera_yaml);
  visod_->setCameraParams(cam_params_);
  visod_->enableMaskForFeatureDetection();

  ros::Publisher cloud_pub_;
  cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(nh_.resolveName("cloud_out"), 5);

  const int MAX_COUNT = 5;
  int counter = MAX_COUNT;
  cv::Mat pose = cv::Mat::eye(4, 4, CV_32F);
  while (getline(ifsL, lineL) && getline(ifsR, lineR)) {
    if (--counter > 0) continue;

    imgL = cv::imread(lineL, 0);
    imgR = cv::imread(lineR, 0);

    visod_->setImages(imgL, imgR);
    //visod_->showImages();

    cv::Mat relative_pose = cv::Mat::eye(4, 4, CV_32F);
    visod_->compute(relative_pose);
    //visod_->showKeyPoints();
    //visod_->showMatches();
    visod_->showTracks(true); // show inliers

    ROS_INFO_STREAM(relative_pose);
    pose = pose * relative_pose;
    const cv::Mat _R = pose(cv::Rect(0, 0, 3, 3));
    Eigen::MatrixXf R;
    convertCV2Eigen<float>(_R, R);
    //TODO 3DOG 

    writeToFile(pose_out_, pose);
    //ROS_INFO_STREAM(pose);
    visod_->publishTF();
#if 0

    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2);
    visod_->getCloudMsg(*cloud_msg);
    cloud_msg->header.stamp = ros::Time::now();
    cloud_msg->header.frame_id = "/camera_left";
    cloud_pub_.publish(cloud_msg);

#endif

    cv::waitKey(1);
    counter = MAX_COUNT;
  }
}
